package edu.tfh.s2.ehkbot.steuerung;

import java.util.Collections;
import java.util.LinkedList;
import java.util.Vector;

/**
 * Die Karte beinhaltet die Infromationen die Nötig sind für das Navigieren.
 * 
 * @author s2zehn
 */
public class Karte {
	/**
	 * Kartenpunkt ist eine Hilfsklasse für das Navigieren. Durch ihre Verlinkte Struktur Speichert sie
	 * Wegpunkte abfolgen.
	 * 
	 * @author s2zehn
	 */
	private class KartenPunkt {
		private int kosten;

		private Position position;
		private KartenPunkt vorgaenger;

		public KartenPunkt(Position position, KartenPunkt vorgaenger, int kosten) {
			super();
			this.position = position;
			this.vorgaenger = vorgaenger;
			this.kosten = kosten;
		}

		/**
		 * Liefert eine Liste mit Nachbarnpunkten in alle 4 Himelsrichtungen.
		 * 
		 * @param entfernung
		 *            Die Entfernung in mm zu den Aktuellen Punkt.
		 * @return Eine Liste mit Nachbarpunkten.
		 */
		public LinkedList<KartenPunkt> getNachbarn(int entfernung) {
			LinkedList<KartenPunkt> nachbarn = new LinkedList<KartenPunkt>();
			int x = position.getX();
			int y = position.getY();
			int z = position.getZ();
			Position nord = null;
			Position ost = null;
			Position sued = null;
			Position west = null;

			nord = new Position(x, y - entfernung, z);

			ost = new Position(x + entfernung, y, z);

			sued = new Position(x, y + entfernung, z);

			west = new Position(x - entfernung, y, z);

			nachbarn.add(new KartenPunkt(nord, this, kosten + 1));
			nachbarn.add(new KartenPunkt(ost, this, kosten + 1));
			nachbarn.add(new KartenPunkt(sued, this, kosten + 1));
			nachbarn.add(new KartenPunkt(west, this, kosten + 1));
			return nachbarn;
		}
	}

	/**
	 * Topologie Speichert den Typ eines Punktes auf der Karte.
	 */
	public enum Toplogie {
		FAHRBEREICH, GESPERT, NITCHVORHANDEN, REGAL
	};

	/**
	 * Mit hilfe von quantiesierung kann man den Rechenaufwand verringern.
	 */
	public static final int quantiesierung = 1000;

	/**
	 * Die Karte als ein 2D Feld von Topologien.
	 */
	private Toplogie[][] topologie;

	/**
	 * Konstruktor für Topologie
	 * 
	 * @param topologie
	 *            Ein Fehl 2D Feld.
	 */
	public Karte(Toplogie[][] topologie) {
		this.topologie = topologie.clone();
	}

	/**
	 * Errechnet den Weg mit hilfe von greedy Algorithmus.
	 * 
	 * @param start
	 *            Der Start Punkt.
	 * @param ende
	 *            Der End Punkt
	 * @return Ein Vektor mit Positionen die den Weg makieren.
	 */
	public Vector<Position> errechneWeg(Position start, Position ende) {
		KartenPunkt startPunkt = new KartenPunkt(start, null, 0);
		/*
		 * neueKordinaten sind Punkte die Noch besucht werden.
		 */
		LinkedList<KartenPunkt> neueKordinaten = startPunkt.getNachbarn(quantiesierung);
		Position positionQuantiesiert = quantiesiere(start);

		/*
		 * schonBesucht sind Punkte die schon mal besucht wurden, das verhindert im Kreis laufem.
		 */
		KartenPunkt[][] schonBesucht = new KartenPunkt[topologie.length][topologie[0].length];
		schonBesucht[positionQuantiesiert.getY()][positionQuantiesiert.getX()] = startPunkt;

		while (neueKordinaten.size() > 0) {
			KartenPunkt punkt = neueKordinaten.pollFirst();
			positionQuantiesiert = quantiesiere(punkt.position);
			schonBesucht[positionQuantiesiert.getY()][positionQuantiesiert.getX()] = punkt;

			Toplogie top = getTopologie(punkt.position);

			if (top == Toplogie.FAHRBEREICH) {
				neueKordinaten.addAll(punkt.getNachbarn(quantiesierung));
			}

			/*
			 * Hier werden schon getestet Position entfernt die Liste wird rückwerts durchschritten damit in
			 * der for schleife gelöscht werden kann, wenn die schleife von 0 bis neueKordinaten.size() gehen
			 * würde, gebe es ein problem wenn man etwas löschen würde, da sich size() verändern würde
			 */
			for (int i = neueKordinaten.size() - 1; i >= 0; --i) {
				positionQuantiesiert = quantiesiere(neueKordinaten.get(i).position);
				try {
					if (schonBesucht[positionQuantiesiert.getY()][positionQuantiesiert.getX()] != null) {
						neueKordinaten.remove(i);
					}
				} catch (java.lang.ArrayIndexOutOfBoundsException exp) {
					neueKordinaten.remove(i);
				}
			}
		}

		positionQuantiesiert = quantiesiere(ende);
		KartenPunkt endePunkt = schonBesucht[positionQuantiesiert.getY()][positionQuantiesiert.getX()];

		/*
		 * Geht die Liste die durch die KartenPunkte gebildet wird durch und überführt sie in ein Vektor.
		 */
		Vector<Position> way = new Vector<Position>();
		KartenPunkt child = endePunkt;
		while (child != null) {
			way.add(child.position);
			child = child.vorgaenger;
		}
		Collections.reverse(way);
		return way;

	}

	/**
	 * Gibt an wie viele mm in einer Richtung von einen Punkt aus gesehen werden kann.
	 * 
	 * @param pos
	 *            Die Start Position.
	 * @param richtung
	 *            Die Sichtrichtung.
	 * @return Die Strecke [mm]
	 */
	public double getAusbreitung(Position pos, Richtung richtung) {
		Toplogie start = getTopologie(pos);
		int x = (int) (Math.round(Karte.quantiesierung * Math.cos(richtung.getAlsRad())));
		int y = (int) (Math.round(Karte.quantiesierung * Math.sin(richtung.getAlsRad())));
		int z = pos.getZ();
		Position toAdd = null;
		double weg = 0;
		toAdd = new Position(x, y, z);
		pos.add(toAdd);
		while (getTopologie(pos).equals(start) && istInKarte(pos)) {
			weg += Karte.quantiesierung;
			pos.add(toAdd);
		}
		return weg;
	}

	/**
	 * Gibt die Topologie für den Punkt an.
	 * 
	 * @param position
	 *            Die Position
	 * @return Die Topologier.
	 */
	public Toplogie getTopologie(Position position) {
		if (istInKarte(position))
			return topologie[position.getY() / quantiesierung][position.getX() / quantiesierung];
		else
			return Toplogie.NITCHVORHANDEN;
	}

	/**
	 * Liefert die X Größe.
	 * 
	 * @return
	 */
	public int getXSize() {
		return topologie[0].length * quantiesierung;
	}

	/**
	 * Liefert die Y Größe.
	 * 
	 * @return
	 */
	public int getYSize() {
		return topologie.length * quantiesierung;
	}

	/**
	 * Prüft ob eine Position in der Karte Liegt.
	 * 
	 * @param position
	 *            Die zu prüfende Position.
	 * @return true fals die Position in der Karte liegt.
	 */
	public boolean istInKarte(Position position) {
		return position.getX() >= 0 && position.getY() >= 0 && position.getX() < getXSize()
						&& position.getY() < getYSize();
	}

	/**
	 * Hilfsfunktion um die Position zu quantiesieren.
	 * 
	 * @param pos
	 *            Die Position.
	 * @return Die quantiesierte Position.
	 */
	private Position quantiesiere(Position pos) {
		int x = Math.round(pos.getX() / quantiesierung);
		int y = Math.round(pos.getY() / quantiesierung);
		int z = Math.round(pos.getZ() / quantiesierung);
		return new Position(x, y, z);
	}
}
